clc
clear
close all;
delay_time = 5;
D2R = pi/180;
fd = fopen("file_8.txt");%%输入文件名称
fseek(fd,0,"eof");
fsize = ftell(fd);
fseek(fd,0,"bof");
num_line = fsize/128;   %128字节每个数据
dataty = zeros(num_line,128);
for i=1:1:(num_line)
    dataty(i,:)=fread(fd,128,"uint8");
end
fclose(fd);
size(dataty)
% disp(dataty);
row_num = find((dataty(:,5) ~=235) | (dataty(:,6) ~=22));   %删除帧头错误的数据
dataty(row_num, :) = [];
size(dataty)
% dataty(length(dataty)*0.7:end, :) = [];
% 时间戳数据
time=dataty(:,1)*16777216+dataty(:,2)*65536+dataty(:,3)*256+dataty(:,4);% 单位1表示10ms；
time = time * 0.01; % 单位为s;
% 地速
FKdata.GroundSpeed = (dataty(:,33)*256+dataty(:,32));
FKdata.GroundSpeed = double(typecast(uint16(FKdata.GroundSpeed),'int16'))*0.1;
% 飞机俯仰角
FKdata.PitchAngle = (dataty(:,23)*256+dataty(:,22));
FKdata.PitchAngle=double(typecast(uint16(FKdata.PitchAngle),'int16'))*0.01;
% 空速
FKdata.AirSpeed = (dataty(:,45)*256+dataty(:,44));
FKdata.AirSpeed = double(typecast(uint16(FKdata.AirSpeed),'int16'))*0.1;
% 对地高度
FKdata.AltitudeToG = (dataty(:,19)*256+dataty(:,18));
FKdata.AltitudeToG=double(typecast(uint16(FKdata.AltitudeToG),'int16'))*0.1;
%FK飞行模式
FKdata.flymode = dataty(:,7);

%DM距离
target_distance = dataty(:,103) * 256 + dataty(:,102);
target_distance = double(typecast(uint16(target_distance),'int16'))*0.1;
row_hkdj = find(FKdata.flymode == 170);
figure();
subplot(2, 2, 1);
plot(time, FKdata.flymode);hold on;grid on;
exit_hkdj_time = time(row_hkdj(end));
x = time(row_hkdj(end-19:end));
y = FKdata.AltitudeToG(row_hkdj(end-19:end));
% 拟合多项式的阶数
order = 1;
% 使用 polyfit 进行多项式拟合
coefficients = polyfit(x, y, order);
exit_hkdj_v = coefficients(1);
subplot(2, 2, 2);
plot(x, y);hold on;grid on;
disp("退出DJ时间:" + num2str(exit_hkdj_time));
disp("退出DJ俯仰角:" + num2str(FKdata.PitchAngle(row_hkdj(end))));
disp("退出DJ空速:" + num2str(FKdata.AirSpeed(row_hkdj(end))));
disp("退出DJ地速:" + num2str(FKdata.GroundSpeed(row_hkdj(end))));
disp("退出DJ垂向速度:" + num2str(exit_hkdj_v));
disp("退出DJ高度:" + num2str(FKdata.AltitudeToG(row_hkdj(end))));
%寻找退出HKDJ10S内的最低高度
min_time = time(row_hkdj(1));
max_time = time(row_hkdj(end)) + delay_time;
row_min = find(time>=min_time);
row_max = find(time<=max_time);
row = intersect(row_min, row_max);
AltitudeToG_min = min(FKdata.AltitudeToG(row));
deta_alt = FKdata.AltitudeToG(row_hkdj(end)) - AltitudeToG_min;
alt_row = FKdata.AltitudeToG(row);
time_row = time(row);
AltitudeToG_min_row = find(alt_row == AltitudeToG_min);
AltitudeToG_min_time = time_row(AltitudeToG_min_row);
deta_alt_time = AltitudeToG_min_time - time(row_hkdj(end));
disp("退出DJ最低高度:" + num2str(AltitudeToG_min));
disp("退出DJ到最低高度时间:" + num2str(deta_alt_time(1)));
subplot(2, 2, 3);
plot(time(row), FKdata.AltitudeToG(row));hold on;
legend("对地高度")
grid on;
subplot(2, 2, 4);
plot(time(row_hkdj), target_distance(row_hkdj));hold on;
plot(time(row_hkdj), FKdata.AltitudeToG(row_hkdj));hold on;
legend("DM距离","对地高度")
grid on;


